/*
clsquare - closed loop simulation system
Copyright (c) 2004, Neuroinformatics Group, Prof. Dr. Martin Riedmiller,
University of Osnabrueck

Author: Martin Riedmiller

All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

  * Redistributions of source code must retain the above copyright
    notice, this list of conditions and the following disclaimer.
  * Redistributions in binary form must reproduce the above copyright
    notice, this list of conditions and the following disclaimer in
    the documentation and/or other materials provided with the
    distribution.
  * Neither the name of the <ORGANIZATION> nor the names of its
    contributors may be used to endorse or promote products derived
    from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "valueparser.h"
#include "str2val.h"
#include "explore.h"
#include <cstdlib>
#include <cmath>
#include <ctime>
#include <fstream>

#define MAX_NUM_ACTIONS 255

#define VERBOSE(xxx) if(verbose) cout <<__PRETTY_FUNCTION__ <<": "<< xxx<<endl

/**********************************************************************/
/* interface to clsquare                                              */
/**********************************************************************/


bool Explore::get_action(const double* observation, double* action)
{

 increment++;
// switch verbosity on if true
 verbose = true;
// the string we use for verbosity output
 char tmp_string[100];

// we increase the dimensions of our representation if nessesary
 if(xdim < observation[0])
 	xdim = observation[0];
 if(ydim < observation[1])
 	ydim = observation[1];

// go_x and go_y are the movements from the last cycle in x or y direction, extracted from the given last action
 int go_x = 0;
 int go_y = 0;
 if(action[0] == 0)
   go_x = -1;
 else if(action[0] == 1)
   go_x = 1;
 else if(action[0] == 2)
   go_y = 1;
 else if(action[0] == 3)
   go_y = -1;
   
// if we stand on a tile which was supposed to be a wall, we reset it
 if(temp_tiles[(int) (1+observation[0])][(int) (1+observation[1])] == -5)
 	temp_tiles[(int) (1+observation[0])][(int) (1+observation[1])] = 0;

// check whether we have moved or not
// if we stood still, the anticipated tile could have been a wall; lets check that again
 if(observation[0] == old_x && observation[1] == old_y)
 {
     temp_tiles[(int) (1+observation[0]+go_x)][(int) (1+observation[1]+go_y)] += 1;
     
     // after we tried this tile unsuccessfully for 8 times, it must be a wall
     if(temp_tiles[(int) (1+observation[0]+go_x)][(int) (1+observation[1]+go_y)] > 9)
     {
     	temp_tiles[(int) (1+observation[0]+go_x)][(int) (1+observation[1]+go_y)] = -5;
     	
     	// surrounding tiles become more attractive if they are not walls
     	if(go_x != 0)
     	{
     		if(temp_tiles[(int) (1+observation[0]+go_x)][(int) (1+observation[1]+1)] > -5)
     			temp_tiles[(int) (1+observation[0]+go_x)][(int) (1+observation[1]+1)] += 1;
     		if(temp_tiles[(int) (1+observation[0]+go_x)][(int) (1+observation[1]-1)] > -5)
     			temp_tiles[(int) (1+observation[0]+go_x)][(int) (1+observation[1]-1)] += 1;
     		if(temp_tiles[(int) (1+observation[0])][(int) (1+observation[1]+1)] > -5)
     			temp_tiles[(int) (1+observation[0])][(int) (1+observation[1]+1)] += 1;
     		if(temp_tiles[(int) (1+observation[0])][(int) (1+observation[1]-1)] > -5)
     			temp_tiles[(int) (1+observation[0])][(int) (1+observation[1]-1)] += 1;
     	}
     	else if(go_y != 0)
     	{
     		if(temp_tiles[(int) (1+observation[0]+1)][(int) (1+observation[1]+go_y)] > -5)
     			temp_tiles[(int) (1+observation[0]+1)][(int) (1+observation[1]+go_y)] += 1;
     		if(temp_tiles[(int) (1+observation[0]-1)][(int) (1+observation[1]+go_y)] > -5)
     			temp_tiles[(int) (1+observation[0]-1)][(int) (1+observation[1]+go_y)] += 1;
     		if(temp_tiles[(int) (1+observation[0]+1)][(int) (1+observation[1])] > -5)
     			temp_tiles[(int) (1+observation[0]+1)][(int) (1+observation[1])] += 1;
     		if(temp_tiles[(int) (1+observation[0]-1)][(int) (1+observation[1])] > -5)
     			temp_tiles[(int) (1+observation[0]-1)][(int) (1+observation[1])] += 1;
     	}
     }
 }
// if we successfully moved in the anticipated direction we increase the cost for the previous tile so we dont return that easily
 else if(old_x+go_x == observation[0] && old_y+go_y == observation[1])
 {
   temp_tiles[(int) (1+observation[0]-go_x)][(int) (1+observation[1]-go_y)] -= 0.1;
 }
 else
 {
// we might want to do something in the case, we moved here by mistake
 }

// updating old x and y coordinates for next cycle
 old_x = observation[0];
 old_y = observation[1];

// steps are the costs for the possible next moves
 double step[4];
 step[0] = temp_tiles[(int) (1+observation[0]-1)][(int) (1+observation[1])];
 step[1] = temp_tiles[(int) (1+observation[0]+1)][(int) (1+observation[1])];
 step[2] = temp_tiles[(int) (1+observation[0])][(int) (1+observation[1]+1)];
 step[3] = temp_tiles[(int) (1+observation[0])][(int) (1+observation[1]-1)];

// look for the minimum of the next possible moves
 double max = step[0];
 for(int j = 0; j < 4; j++)
   if(step[j] > max) max = step[j];

// go_there are the possible minima
 int go_there[4];

// k is the number of minima
 int k = 0;

// all steps with the minimum costs are possible actions in go_there[]
 for(int m = 0; m < 4; m++)
 {
   if(step[m] == max) go_there[k++] = m;
 }

// work around because filling action[0] just did not work!
 int action_value[action_dim];

// seeding the random numbers
 srand(time(0));
// someone forgot to seed srand48 in the maze.cpp
 srand48(time(0));

// chose one of the possible actions for the next move
 for (int l = 0; l < action_dim; l++) {
   action_value[l] = (int) (go_there[(int) (rand()%k)]);
   action[l] = action_def[l*MAX_NUM_ACTIONS+action_value[l]];
 }

 // print which run we are in
 sprintf(tmp_string,"Run %i ", increment++);
 VERBOSE(""<<tmp_string);

 // the actual position
 sprintf(tmp_string,"[x,y] = [%g,%g]", observation[0], observation[1]);
 VERBOSE(""<<tmp_string);

 // the next possible moves printed in verbosity
 sprintf(tmp_string,"step[0], [1], [2], [3] = %g, %g, %g, %g", step[0], step[1], step[2], step[3]);
 VERBOSE(""<<tmp_string);

 // print the chosen action
 sprintf(tmp_string,"action_value[0] = %i ", action_value[0]);
 VERBOSE("action "<<tmp_string);

 return true;
}

bool Explore::deinit(){
 VERBOSE("finishing");
 return true;
}

void Explore::notify_episode_starts(const double* initial_observation){     
 VERBOSE("Episode starts");
};

void Explore::notify_episode_stops(const double* final_observation, double final_reward, bool is_terminal_state){

 int unexplored_tiles_num = 0;
 ofstream output;
 output.open("out.def");
 
 if(is_terminal_state)
 	temp_tiles[(int) (1+final_observation[0])][(int) (1+final_observation[1])] = 10;
 	
 for(int l = ydim+2; l >= 0; l--)
 {
    for(int k = 0; k < xdim+3; k++)
    {
    	if(temp_tiles[k][l] == 0)
    		unexplored_tiles_num++;
    	if(temp_tiles[k][l] > -5 && temp_tiles[k][l] < 10)
    		temp_tiles[k][l] = -0.1;
    	
    	if(temp_tiles[k][l] < -4)
    		output<<'X';
    	else if(temp_tiles[k][l] > 9)
    		output<<'G';
    	else
    		output<<'.';
    }
    output<<endl;
 }
 //stats file
 ofstream stats_file;
 stats_file.open("stats.dat");
 stats_file<<"#updates of value function   average steps to goal"<<endl;
 if(unexplored_tiles_num > 10)
 	stats_file<<"#done after "<<increment<<" steps"<<endl;
 
 stats_file.close();
 output.close();
 VERBOSE("Episode stops");
};

bool Explore::init(const int observation_dim, const int action_dim, double deltat, const char* fname, const char* chapter)
{
 this->observation_dim = observation_dim;
 this->action_dim = action_dim;
 verbose = false;
 xdim = 2;
 ydim = 2;

 bool result = read_options(fname);
 srand48(234);
 VERBOSE("init: got observed_observation_dim: "<<observation_dim<<" and action_dim:"<<action_dim<<" and delta_t: "<<deltat);
 return result;
}

bool Explore::read_options(const char * fname) {
 char param[255];

 ValueParser vp(fname,"Controller");
 vp.get("verbose",verbose);
 vp.get("actions",param,255);
 parse_actions(param);

 return true;
}

void Explore::parse_actions(const char* param)
{     const char* str = param;
 double val;
 action_def = new double[MAX_NUM_ACTIONS*action_dim];   actions_in_dim = new int[action_dim];

 for (int i = 0; i<action_dim;i++){     strskip(str,"[",str);        int j = 0;
   while(str2val(str,val,str)) {          action_def[i*MAX_NUM_ACTIONS+j] = val;          j++;       }          actions_in_dim[i] = j;      strskip(str,"]",str);
 }                }


REGISTER_CONTROLLER(Explore, "By far the most advanced agent out there.")
